#ifndef _NE_MATRIX4_H_
#define _NE_MATRIX4_H_

#include <mathematics/Matrix3.h>
#include <mathematics/Vector4.h>
#include <mathematics/Quaternion.h>

namespace ne
{
    /*
        Matrix4 is the stander math matrix
        v' = v * Matrix4;

        TODO: The Vector or Matrix at the left of '*' will be rotated by
        Matrix at the right
    */

    class NEAPI Matrix4
    {
        public:
            inline Matrix4()
            {
                mData[0][0] = 1.0f; mData[0][1] = 0.0f; mData[0][2] = 0.0f; mData[0][3] = 0.0f;
                mData[1][0] = 0.0f; mData[1][1] = 1.0f; mData[1][2] = 0.0f; mData[1][3] = 0.0f;
                mData[2][0] = 0.0f; mData[2][1] = 0.0f; mData[2][2] = 1.0f; mData[2][3] = 0.0f;
                mData[3][0] = 0.0f; mData[3][1] = 0.0f; mData[3][2] = 0.0f; mData[3][3] = 1.0f;
            }

            inline Matrix4(const Matrix4 &m)
            {
                memcpy(mData, m.mData, 16 * sizeof(real));
            }

            inline explicit Matrix4(const Matrix3 &m)
            {
                mData[0][0] = m[0][0]; mData[0][1] = m[0][1]; mData[0][2] = m[0][2]; mData[0][3] = 0.0f;
                mData[1][0] = m[1][0]; mData[1][1] = m[1][1]; mData[1][2] = m[1][2]; mData[1][3] = 0.0f;
                mData[2][0] = m[2][0]; mData[2][1] = m[2][1]; mData[2][2] = m[2][2]; mData[2][3] = 0.0f;
                mData[3][0] = 0.0f; mData[3][1] = 0.0f; mData[3][2] = 0.0f; mData[3][3] = 1.0f;
            }

            inline explicit Matrix4(const real m[4][4])
            {
                memcpy(mData, m, 16 * sizeof(real));
            }

            inline explicit Matrix4(const real m[16])
            {
                memcpy(mData, m, 16 * sizeof(real));
            }

            inline Matrix4(const real m00, const real m01, const real m02, const real m03,
                           const real m10, const real m11, const real m12, const real m13,
                           const real m20, const real m21, const real m22, const real m23,
                           const real m30, const real m31, const real m32, const real m33)
            {
                mData[0][0] = m00; mData[0][1] = m01; mData[0][2] = m02; mData[0][3] = m03;
                mData[1][0] = m10; mData[1][1] = m11; mData[1][2] = m12; mData[1][3] = m13;
                mData[2][0] = m20; mData[2][1] = m21; mData[2][2] = m22; mData[2][3] = m23;
                mData[3][0] = m30; mData[3][1] = m31; mData[3][2] = m32; mData[3][3] = m33;
            }

            inline void swap(Matrix4 &m)
            {
                std::swap(mData[0][0], m.mData[0][0]);
                std::swap(mData[0][1], m.mData[0][1]);
                std::swap(mData[0][2], m.mData[0][2]);
                std::swap(mData[0][3], m.mData[0][3]);
                std::swap(mData[1][0], m.mData[1][0]);
                std::swap(mData[1][1], m.mData[1][1]);
                std::swap(mData[1][2], m.mData[1][2]);
                std::swap(mData[1][3], m.mData[1][3]);
                std::swap(mData[2][0], m.mData[2][0]);
                std::swap(mData[2][1], m.mData[2][1]);
                std::swap(mData[2][2], m.mData[2][2]);
                std::swap(mData[2][3], m.mData[2][3]);
                std::swap(mData[3][0], m.mData[3][0]);
                std::swap(mData[3][1], m.mData[3][1]);
                std::swap(mData[3][2], m.mData[3][2]);
                std::swap(mData[3][3], m.mData[3][3]);
            }

            inline void setRow(const size_t r, const Vector4 &v)
            {
                assert(r < 4);
                mData[r][0] = v.x;
                mData[r][1] = v.y;
                mData[r][2] = v.z;
                mData[r][3] = v.w;
            }

            inline Vector4 getRow(const size_t r) const
            {
                assert(r < 4);
                return Vector4(mData[r][0], mData[r][1], mData[r][2], mData[r][3]);
            }

            inline void setColumn(const size_t c, const Vector4 &v)
            {
                assert(c < 4);
                mData[0][c] = v.x;
                mData[1][c] = v.y;
                mData[2][c] = v.z;
                mData[3][c] = v.w;
            }

            inline Vector4 getColumn(const size_t c) const
            {
                assert(c < 4);
                return Vector4(mData[0][c], mData[1][c], mData[2][c], mData[3][c]);
            }

            inline real* operator [] (const size_t v)
            {
                assert(v < 4);
                return mData[v];
            }

            inline const real* operator [] (const size_t v) const
            {
                assert(v < 4);
                return mData[v];
            }

            inline real* ptr()
            {
                return mData[0];
            }

            inline const real* ptr() const
            {
                return mData[0];
            }

            inline Matrix4& operator = (const Matrix4 &m)
            {
                memcpy(mData, m.mData, 16 * sizeof(real));
                return *this;
            }

            inline void identity()
            {
                mData[0][0] = 1.0f; mData[0][1] = 0.0f; mData[0][2] = 0.0f; mData[0][3] = 0.0f;
                mData[1][0] = 0.0f; mData[1][1] = 1.0f; mData[1][2] = 0.0f; mData[1][3] = 0.0f;
                mData[2][0] = 0.0f; mData[2][1] = 0.0f; mData[2][2] = 1.0f; mData[2][3] = 0.0f;
                mData[3][0] = 0.0f; mData[3][1] = 0.0f; mData[3][2] = 0.0f; mData[3][3] = 1.0f;
            }

            inline void fromTransform(const Vector3 &v)
            {
                mData[0][0] = 1.0f; mData[0][1] = 0.0f; mData[0][2] = 0.0f; mData[0][3] = 0.0f;
                mData[1][0] = 0.0f; mData[1][1] = 1.0f; mData[1][2] = 0.0f; mData[1][3] = 0.0f;
                mData[2][0] = 0.0f; mData[2][1] = 0.0f; mData[2][2] = 1.0f; mData[2][3] = 0.0f;
                mData[3][0] = v.x;  mData[3][1] = v.y;  mData[3][2] = v.z;  mData[3][3] = 1.0f;
            }

            inline void setTransform(const Vector3 &v)
            {
                mData[3][0] = v.x;
                mData[3][1] = v.y;
                mData[3][2] = v.z;
            }

            inline void fromScale(const Vector3 &v)
            {
                mData[0][0] = v.x;  mData[0][1] = 0.0f; mData[0][2] = 0.0f; mData[0][3] = 0.0f;
                mData[1][0] = 0.0f; mData[1][1] = v.y;  mData[1][2] = 0.0f; mData[1][3] = 0.0f;
                mData[2][0] = 0.0f; mData[2][1] = 0.0f; mData[2][2] = v.z;  mData[2][3] = 0.0f;
                mData[3][0] = 0.0f; mData[3][1] = 0.0f; mData[3][2] = 0.0f; mData[3][3] = 1.0f;
            }

            inline void setScale(const Vector3 &v)
            {
                mData[0][0] = v.x;
                mData[1][1] = v.y;
                mData[2][2] = v.z;
            }

            inline void fromRotationMatrix(const Matrix3 &m)
            {
                mData[0][0] = m[0][0]; mData[0][1] = m[0][1]; mData[0][2] = m[0][2]; mData[0][3] = 0.0f;
                mData[1][0] = m[1][0]; mData[1][1] = m[1][1]; mData[1][2] = m[1][2]; mData[1][3] = 0.0f;
                mData[2][0] = m[2][0]; mData[2][1] = m[2][1]; mData[2][2] = m[2][2]; mData[2][3] = 0.0f;
                mData[3][0] = 0.0f;    mData[3][1] = 0.0f;    mData[3][2] = 0.0f;    mData[3][3] = 1.0f;
            }

            inline void setRotationMatrix(const Matrix3 &m)
            {
                mData[0][0] = m[0][0]; mData[0][1] = m[0][1]; mData[0][2] = m[0][2];
                mData[1][0] = m[1][0]; mData[1][1] = m[1][1]; mData[1][2] = m[1][2];
                mData[2][0] = m[2][0]; mData[2][1] = m[2][1]; mData[2][2] = m[2][2];
            }

            inline void toRotationMatrix(Matrix3 &m) const
            {
                m[0][0] = mData[0][0]; m[0][1] = mData[0][1]; m[0][2] = mData[0][2];
                m[1][0] = mData[1][0]; m[1][1] = mData[1][1]; m[1][2] = mData[1][2];
                m[2][0] = mData[2][0]; m[2][1] = mData[2][1]; m[2][2] = mData[2][2];
            }

            inline void fromQuaternion(const Quaternion &q)
            {
                Matrix3 m;
                q.toRotationMatrix(m);
                fromRotationMatrix(m);
            }

            inline void toQuaternion(Quaternion &q) const
            {
                Matrix3 m;
                toRotationMatrix(m);
                q.fromRotationMatrix(m);
            }

            // TODO: Local to World: TM = S * R * T
            inline void makeTransformMatrix(const Vector3 &position, const Quaternion &orientation, const Vector3 &scale=Vector3::ONE)
            {
                Matrix3 rotM;
                orientation.toRotationMatrix(rotM);
                mData[0][0] = scale.x * rotM[0][0]; mData[0][1] = scale.x * rotM[0][1]; mData[0][2] = scale.x * rotM[0][2]; mData[0][3] = 0.0f;
                mData[1][0] = scale.y * rotM[1][0]; mData[1][1] = scale.y * rotM[1][1]; mData[1][2] = scale.y * rotM[1][2]; mData[1][3] = 0.0f;
                mData[2][0] = scale.z * rotM[2][0]; mData[2][1] = scale.z * rotM[2][1]; mData[2][2] = scale.z * rotM[2][2]; mData[2][3] = 0.0f;
                mData[3][0] = position.x;           mData[3][1] = position.y;           mData[3][2] = position.z;           mData[3][3] = 1.0f;
            }

            // TODO: World to Local:TM' = T' * R' * S'
            inline void makeInverseTransformMatrix(const Vector3 &position, const Quaternion &orientation, const Vector3 &scale=Vector3::ONE)
            {
                assert(scale.x != 0.0f && scale.y != 0.0f && scale.z != 0.0f);

                Vector3 invPosition = -position;
                Quaternion invOrientation = orientation.inverse();
                Vector3 invScale(1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z);

                // Rotate
                invPosition = invOrientation * invPosition;
                // Scale
                invPosition *= invScale;

                Matrix3 rotM;
                invOrientation.toRotationMatrix(rotM);
                mData[0][0] = invScale.x * rotM[0][0]; mData[0][1] = invScale.x * rotM[0][1]; mData[0][2] = invScale.x * rotM[0][2]; mData[0][3] = 0.0f;
                mData[1][0] = invScale.y * rotM[1][0]; mData[1][1] = invScale.y * rotM[1][1]; mData[1][2] = invScale.y * rotM[1][2]; mData[1][3] = 0.0f;
                mData[2][0] = invScale.z * rotM[2][0]; mData[2][1] = invScale.z * rotM[2][1]; mData[2][2] = invScale.z * rotM[2][2]; mData[2][3] = 0.0f;
                mData[3][0] = invPosition.x;           mData[3][1] = invPosition.y;           mData[3][2] = invPosition.z;           mData[3][3] = 1.0f;

            }

            inline bool isAffine() const
            {
                return (mData[0][3] == 0.0f) && (mData[1][3] == 0.0f) && (mData[2][3] == 0.0f) && (mData[3][3] == 1.0f);
            }

            bool operator == (const Matrix4 &m) const;
            bool operator != (const Matrix4 &m) const;

            Matrix4 operator + (const Matrix4 &m) const;
            Matrix4 operator - (const Matrix4 &m) const;
            Matrix4 operator * (const Matrix4 &m) const;

            Matrix4 operator - () const;

            // M * V3
            Vector3 operator * (const Vector3 &v) const;
            // M * V4
            Vector4 operator * (const Vector4 &v) const;
            // TODO: Use this to rotate Vector3
            // V3 * M   (V3 * M) == (T[M] * V3) != (M * V3)
            NEAPI friend Vector3 operator * (const Vector3 &v, const Matrix4 &m);
            // TODO: Use this to rotate Vector4
            // V4 * M   (V4 * M) == (T[M] * V4) != (M * V4)
            NEAPI friend Vector4 operator * (const Vector4 &v, const Matrix4 &m);

            Matrix4 operator * (const real f) const;
            NEAPI friend Matrix4 operator * (const real f, const Matrix4 &m);

            // m = T[M];
            void transpose(Matrix4 &m) const;
            // T[M]
            Matrix4 transpose() const;

            // m = adj(M) = transpose(c(M))
            void adjoint(Matrix4 &m) const;
            // adj(M) = transpose(c(M))
            Matrix4 adjoint() const;

            // m = 1 / M
            bool inverse(Matrix4 &m) const;
            // 1 / M
            Matrix4 inverse() const;

            // |M|
            real determinant() const;

            NEAPI friend std::ostream& operator << (std::ostream &o, const Matrix4 &m);

            inline ~Matrix4() {}

        public:
            inline static real Minor(const Matrix4 &m,
                                     const size_t r0, const size_t r1, const size_t r2,
                                     const size_t c0, const size_t c1, const size_t c2)
            {
                return m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) -
                       m[r0][c1] * (m[r1][c0] * m[r2][c2] - m[r2][c0] * m[r1][c2]) +
                       m[r0][c2] * (m[r1][c0] * m[r2][c1] - m[r2][c0] * m[r1][c1]);
            }

        public:
            static const Matrix4 ZERO;
            static const Matrix4 IDENTITY;
            static const Matrix4 CLIP_SPACE_2D_TO_IMAGE_SPACE;

        private:
            real mData[4][4];
    };

    typedef std::vector<Matrix4> Matrix4Vector;
    typedef std::vector<Matrix4>::iterator Matrix4VectorIterator;
    typedef std::vector<Matrix4>::const_iterator Matrix4VectorConstIterator;
}
#endif
